#include "polygonbody.h"
#include "b2Separator.h"
#include "forcewrapper.h"

PolygonBody::PolygonBody(b2World *world, b2BodyType body_type,
                         std::vector<b2Vec2> &points,
                         b2Vec2 pivot_center,
                         float angle_in_radius,
                         float density,
                         float friction)
    : BodyWrapper(world, b2Shape::e_polygon, body_type, pivot_center, angle_in_radius, density, friction)
{
    std::copy(points.begin(), points.end(), this->points.begin());
    createBody();
}

PolygonBody::PolygonBody(b2World *world,
                         b2BodyType body_type,
                         const b2Vec2 *points,
                         int size,
                         b2Vec2 pivot_center,
                         float angle_in_radius,
                         float density,
                         float friction)
    : BodyWrapper(world, b2Shape::e_polygon, body_type, pivot_center, angle_in_radius, density, friction)
{

    this->points.reserve(size);
    for(int i = 0; i < size; ++ i) {
        this->points.push_back(points[i]);
    }
    createBody();
}

void PolygonBody::draw(QPainter *painter)
{
    painter->save();
    b2Vec2 position = body->GetPosition();
    painter->translate(position.x, position.y);
    ForceWrapper *force;
    b2Vec2 force_direction;
    for(auto iter = forces.begin(); iter != forces.end(); ++ iter) {
        force = iter->second;
        force_direction = force->getForce();
        painter->drawLine(QPointF(0, 0), QPointF(force_direction.x, force_direction.y));
    }

    painter->rotate(body->GetAngle() * 180 / M_PI);
    QPolygonF polygon;
    for(b2Vec2 p : points) {
        polygon << QPointF(p.x, p.y);
    }
    painter->drawPolygon(polygon);
    painter->restore();
    BodyWrapper::draw(painter);
}

bool PolygonBody::inRegion(int top, int left, int right, int bottom)
{
    bool in_region = false;
    b2Vec2 pos = getPosition();
    for(std::vector<b2Vec2>::iterator iter = points.begin(); iter != points.end(); ++ iter) {
        b2Vec2 vertex = pos;
        vertex += *iter;
        if(pointInRegion(vertex, top, left, right, bottom)) {
            in_region = true;
            break;
        }
    }
    return in_region;
}

void PolygonBody::createBody()
{
    b2BodyDef bodyDef;
    bodyDef.type = body_type;
    bodyDef.position = pivot_center;
    bodyDef.angle = angle_in_radius;
    body = world->CreateBody(&bodyDef);
    b2PolygonShape shape;
    b2FixtureDef bodyFixtureDef;
    bodyFixtureDef.density = density;
    bodyFixtureDef.friction = friction;
    b2Separator sep;
    sep.Separate(body, &bodyFixtureDef, &points, 1);
    body->CreateFixture(&bodyFixtureDef);
}
